#include "chassis.h"
#include "can_motor_user.h"
#include "math.h"
#include "basicmath.h"
#define PI   3.14159265f
#define Radius 0.60f
#define Reduction_ratio 18.0f

#define min_abs(x,y) ( (ABS(x) <= ABS(y))? ABS(x) : ABS(y) )
#define four_min_abs(x,y,z,w)  ( min_abs(min_abs(x,y),min_abs(z,w)) )


float angle_to_radian = 0.01745f;  //角度转弧度
float speed[4]={0,0,0,0};
extern DJI_3508   m_0x201_can1;       //底盘3508
extern DJI_3508   m_0x202_can1;       //底盘3508
extern DJI_3508   m_0x203_can1;       //底盘3508
extern DJI_3508   m_0x204_can1;       //底盘3508
extern DJI_3508   m_0x205_can1;       //防守3508

extern DJI_3508   m_0x201_can2;       //运球3508
extern DJI_3508   m_0x202_can2;       //运球3508
extern DJI_3508   m_0x203_can2;       //投篮3508
extern DJI_3508   m_0x204_can2;       //投篮3508
extern DJI_2006   m_0x205_can2;       //投篮2006

/*--------------------------------------------------*/
/**
 * @brief   全部电机停止输出
 */
void all_stop(void)
{
    m_0x201_can1.set_stop();
    m_0x202_can1.set_stop();
    m_0x203_can1.set_stop();
    m_0x204_can1.set_stop();
    m_0x205_can1.set_stop();
    m_0x201_can2.set_stop();
    m_0x202_can2.set_stop();
    m_0x203_can2.set_stop();
    m_0x204_can2.set_stop();
    m_0x205_can2.set_stop();
}

/**
 * @brief 全向轮底盘解算一（相对坐标系）
 * @param Vx		X轴期望速度
 * @param Vy		Y轴期望速度
 * @param Vz 		Z轴期望速度
 */
void getValues(float Vx ,float Vy, float Vz)
{
    float circleLenth = 2*PI*Radius;//底盘周长

    speed[0]= (-Vx*cos(45*angle_to_radian) + Vy*cos(45*angle_to_radian)+Vz*Radius)/circleLenth*Reduction_ratio*60.0;
    speed[1]= (-Vx*cos(45*angle_to_radian) - Vy*cos(45*angle_to_radian)+Vz*Radius)/circleLenth*Reduction_ratio*60.0;
    speed[2]= (+Vx*cos(45*angle_to_radian) - Vy*cos(45*angle_to_radian)+Vz*Radius)/circleLenth*Reduction_ratio*60.0;
    speed[3]= (+Vx*cos(45*angle_to_radian) + Vy*cos(45*angle_to_radian)+Vz*Radius)/circleLenth*Reduction_ratio*60.0;

    //针对全向轮走不直进行修正
    {
//        speed[0] *= 0.98f;
        //	speed[1] *= 0.9f;
//        speed[2] *= 0.98f;
        //	speed[3] *= 0.9f;
    }

    m_0x201_can1.set_speed(speed[0]);
    m_0x202_can1.set_speed(speed[1]);
    m_0x203_can1.set_speed(speed[2]);
    m_0x204_can1.set_speed(speed[3]);
}


